#include <stdio.h>
#include "pid.h"

//PID结构体初始化
void PID_Init(float KP,float KI,float KD,PIDController  *PID)
{
   PID->kp = KP;
   PID->ki = KI;
   PID->kd = KD;
   PID->ek = 0.0f;	
   PID->ek1 = 0.0f;
   PID->ek2 = 0.0f;
   PID->location_sum = 0.0f;
   PID->out = 0.0f;
}

//位置式PID
#define PID_Limit_min   -10000  //PID输出最小值   其实它是I_Out项来着.
#define PID_Limit_max    10000  //PID输出最大值
float PID_location(float setvalue, float actualvalue,PIDController  *PID)
{
  PID->ek = setvalue - actualvalue ;
	PID->location_sum  += PID->ek ;
  if((PID->ki!=0)&&PID->location_sum>(PID_Limit_max/PID->ki)) PID->location_sum = PID_Limit_max/PID->ki ; 
  if((PID->ki!=0)&&PID->location_sum<(PID_Limit_min/PID->ki)) PID->location_sum = PID_Limit_min/PID->ki ;  //积分限幅.   //这里感觉要另设一个I_Limit_max/min,来除以ki,但不管先留着注释一下
	
	 PID->out = PID->kp* PID->ek + (PID->ki* PID->location_sum) + PID->kd* (PID->ek-PID->ek1 );
	 PID->ek1 = PID->ek;
	
	return PID->out;
	
}
//增量式PID
float PID_increment(float setvalue, float actualvalue,PIDController *PID)
{
	  PID->ek = setvalue - actualvalue ;
    PID->out = PID->kp* (PID->ek - PID->ek1 )+ PID->ki* PID->ek + PID->kd*(PID->ek- 2* PID->ek1 +PID->ek2 );
	
	  PID->ek = PID->ek1;
	  PID->ek1 = PID->ek2;
	
	 if(PID->out>PID_Limit_max)  PID->out = PID_Limit_max;
	 if(PID->out<PID_Limit_min)  PID->out = PID_Limit_min;

	return PID->out;
} 
volatile int16_t  error_change;

//以上两个函数是通用性较强的,下面再分享一些关于电机控制的PID算法.
int16_t Left_pwm_PID(int16_t setspeed,int16_t encoder)
{
	static int16_t pwmout = 0,error = 0, last_error = 0;  // static(被申明的变量就成静态变量,)程序执行结束,值不会被销毁,会以上一次最后的值保存着.

	float Kp = 1.0f , Ki = 1.0f ,Kd = 0.0f;
	error = setspeed - encoder ;
	
	pwmout = pwmout + Kp* (error - last_error) + Ki * error ;
	error_change = error - last_error;

	last_error =  error ;
	
	if(pwmout>4000)   pwmout =4000;
	if(pwmout<-4000)  pwmout =-4000;
	
	return pwmout;	
}
int16_t Right_pwm_PID(int16_t setspeed,int16_t encoder)
{
	static int16_t pwmout = 0,error = 0, last_error = 0;  // static(被申明的变量就成静态变量,)程序执行结束,值不会被销毁,会以上一次最后的值保存着.

	float Kp = 1.0f , Ki = 1.0f ,Kd = 0.0f;
	error = setspeed - encoder ;
	
	pwmout = pwmout + Kp* (error - last_error) + Ki * error ;
	error_change = error - last_error;

	last_error =  error ;
	
	if(pwmout>4000)   pwmout =4000;
	if(pwmout<-4000)  pwmout =-4000;
	
	return pwmout;	
}
